;*****************************************************************************
;
; RTOS.s
; 
; Brian Bonnet
; Steven Eckoff
;
; Initial Creation: February  2, 2011
; Last Modified:    February 11, 2011
; 
; This contains all the assembly language functions used in our OS. This file
; has handles launching the OS, context switching, and 
;
; FOR LAB 01
;
; TA: Karthik Sankar
;
; Hardware Configuration: None
;
;*****************************************************************************

  EXTERN CurrentThreadPt
  EXTERN FirstSwitchOccurred
  EXTERN OS_FindNextThread
  
  EXPORT OS_CPU_RunFirstContext
  EXPORT OS_CPU_ContextSwitch
  EXPORT OS_CPU_PendSVHandler

NVIC_INT_CTRL   EQU     0xE000ED04                              ; Interrupt control state register.
NVIC_PENDSVSET  EQU     0x10000000                              ; Value to trigger PendSV exception.

  AREA |.text|, CODE, READONLY, ALIGN=2
    THUMB
    REQUIRE8
    PRESERVE8



OS_CPU_ContextSwitch
        LDR R0, =NVIC_INT_CTRL
		LDR R1, =NVIC_PENDSVSET
		STR R1, [R0]
		BX  LR

OS_CPU_RunFirstContext
		LDR     R0,  =CurrentThreadPt
		LDR     R0,  [R0]
        LDR     R0,  [R0, #8]                                        ; R0 is new process SP; SP = OSTCBHighRdy->OSTCBStkPtr;
        LDM     R0,  {R4-R11}                                        ; Restore r4-11 from new process stack
        ADDS    R0,  R0, #0x24
		LDM     R0,  {R1-R3}										 ; Restore r1-3
		ADDS	R0,  R0, #0x0C
		LDR     R13, [R0, #4]										 ; Restore LR
		LDR     R12, [R0, #8]										 ; Save thread address into R12
		ADDS    R0,  R0, #12										 ; Move R0 to end of stack
		MOV     R13,  R0										     ; Load new stack value into SP
        BX      R12                                                  ; Jump to thread

OSStartHang
        B      OSStartHang                                           ; This line should never be executed

	
;********************************************************************************************************
;                                         HANDLE PendSV EXCEPTION
;                                     void OS_CPU_PendSVHandler(void)
;
; Note(s) : 1) PendSV is used to cause a context switch.  This is a recommended method for performing
;              context switches with Cortex-M3.  This is because the Cortex-M3 auto-saves half of the
;              processor context on any exception, and restores same on return from exception.  So only
;              saving of R4-R11 is required and fixing up the stack pointers.  Using the PendSV exception
;              this way means that context saving and restoring is identical whether it is initiated from
;              a thread or occurs due to an interrupt or exception.
;
;           2) Pseudo-code is:
;              a) Get the process SP, if 0 then skip (goto d) the saving part (first context switch);
;              b) Save remaining regs r4-r11 on process stack;
;              c) Save the process SP in its TCB, OSTCBCur->OSTCBStkPtr = SP;
;              d) Call OSTaskSwHook();
;              e) Get current high priority, OSPrioCur = OSPrioHighRdy;
;              f) Get current ready thread TCB, OSTCBCur = OSTCBHighRdy;
;              g) Get new process SP from TCB, SP = OSTCBHighRdy->OSTCBStkPtr;
;              h) Restore R4-R11 from new process stack;
;              i) Perform exception return which will restore remaining context.
;
;           3) On entry into PendSV handler:
;              a) The following have been saved on the process stack (by processor):
;                 xPSR, PC, LR, R12, R0-R3
;              b) Processor mode is switched to Handler mode (from Thread mode)
;              c) Stack is Main stack (switched from Process stack)
;              d) OSTCBCur      points to the OS_TCB of the task to suspend
;                 OSTCBHighRdy  points to the OS_TCB of the task to resume
;
;           4) Since PendSV is set to lowest priority in the system (by OSStartHighRdy() above), we
;              know that it will only be run when no other exception or interrupt is active, and
;              therefore safe to assume that context being switched out was using the process stack (PSP).
;********************************************************************************************************	
OS_CPU_PendSVHandler
        CPSID   I                                                   ; Prevent interruption during context switch

		LDR     R1, =CurrentThreadPt                                ; R1 = &(CurrentThreadPt->ControlBlock.StackPt)
        LDR     R1, [R1]
		ADDS    R1, R1, #8
        
		MOV     R0, R13                                             ; R0 = SP
		SUBS    R0, R0, #0x20                                       ; Save remaining regs r4-11 on process stack
        STM     R0, {R4-R11}

        
        STR     R0, [R1]                                            ; Save new SP into TCB
		MOV		R4, R14
		LDR     R3, =OS_FindNextThread							; Update CurrentThreadPt
		BLX     R3
		MOV     R14, R4	

        LDR     R0, =CurrentThreadPt                                ; R0 holds address of CurrentThreadPt
		LDR     R1, [R0]											; R1 holds address of actual current TCB 

        LDR     R0, [R1, #8]                                        ; R0 is new process SP; SP = OSTCBHighRdy->OSTCBStkPtr;
        LDM     R0, {R4-R11}                                        ; Restore r4-11 from new process stack
        ADDS    R0, R0, #0x20
        MOV     R13, R0                                             ; Load MSP with new process SP
        ORR     LR, LR, #0x09                                       ; Ensure exception return uses process stack
        CPSIE   I
        BX      LR                                                  ; Exception return will restore remaining context

        END